// Devin Koepl

#include <fca_controllers/controller.h>

extern void initialize_motor_position_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.;
}


extern void update_motor_position_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 	- MTR_POS_CONTROLLER_DATA(data)->p_gain * (MTR_POS_CONTROLLER_DATA(data)->mtr_ang - input->motor_angle) 
							+ MTR_POS_CONTROLLER_DATA(data)->d_gain * input->motor_velocity;
		
}


extern void takedown_motor_position_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.;
}
